package io.zjw.testblelib.util;

import java.util.ArrayList;

/**
 * 零相位滤波函数
 * 
 * @author 黄大卫,胡盼
 * @Email hhhdddwww@163.com
 * @version 1.0
 * @Time 2016年3月23日下午9:04:28
 * @Description 调用static方法：doFiltfilt执行滤波
 * @copyright 安徽大学 iiphci 实验室
 * 
 */

public class Filtfilt {


	public static void main(String[] args) {
		double []  signal = {4295,19310,19643,19613,19585,19453,19523,19411,19415,19451,19474,19424,19497,19441,19626,19693,19623,19782,19794,19747,19782,19749,19785,19811,19731,19744,19829,19790,19861,19874,19836,19912,19870,19997,19961,20028,20072,20067,20116,20087,20151,20163,20174,20227,20205,20207,20281,20268,20244,20314,20331,20288,20341,20328,20395,20407,20450,20464,20498,20514,20569,20505,20451,20571,20629,20488,20444,20391,20256,20156,20002,19934,19789,19700,19597,19562,19527,19594,19575,19504,19507,19488,19474,19559,19486,19497,19487,19489,19607,19579,19565,19691,19664,19707,19722,19707,19732,19739,19752,19751,19717,19733,19742,19703,19714,19711,19715,19746,19760,19746,19818,19790,19889,19884,19857,19870,19892,19872,19911,19952,19924,19949,19964,19975,19952,19995,20049,20087,20103,20093,20139,20080,20109,20099,20113,20164,20171,20159,20222,20222,20229,20221,20236,20246,20158,20148,20061,19966,19826,19670,19592,19469,19481,19289,19252,19270,19217,19175,19156,19110,19183,19159,19106,19229,19118,19121,19174,19198,19230,19222,19227,19308,19392,19345,19418,19370,19348,19444,19370,19360,19355,19333,19312,19361,19393,19405,19418,19434,19451,19438,19407,19449,19538,19583,19608,19547,19516,19621,19571,19674,19668,19681,19778,19773,19730,19735,19700,19776,19851,19793,19865,19864,19836,19922,19868,19916,19950,19971,19995,19950,19992,20060,20070,20065,20062,19963,19883,19814,19613,19492,19430,19318,19260,19191,19102,19148,19092,19100,19093,19084,19092,19145,19081,19077,19054,19054,19151,19220,19261,19261,19231,19303,19311,19256,19352,19332,19305,19350,19355,19338,19447,19329,19413,19414,19387,19449,19451,19446,19500,19468,19513,19477,19491,19648,19614,19643,19666,19694,19726,19836,19773,19779,19770,19780,19813,19820,19861,19867,19835,19894,19819,19848,19960,19992,19968,19999,20033,20076,20080,20065,20018,20091,20046,20089,19968,19878,19845,19711,19598,19587,19412,19376,19429,19357,19358,19308,19307,19352,19332,19309,19308,19315,19410,19421,19429,19442,19422,19485,19585,19614,19589,19636,19661,19656,19674,19623,19666,19609,19668,19681,19632,19685,19703,19711,19685,19713,19662,19781,19775,19846,19832,19810,19816,19904,19815,19942,19919,20041,20099,20019,20113,20082,20068,20100,20145,20127,20187,20136,20236,20186,20219,20243,20259,20262,20299,20267,20296,20359,20251,20361,20386,20394,20515,20529,20454,20490,20373,20386,20295,20100,19886,19890,19785,19684,19629,19589,19556,19497,19594,19496,19544,19502,19555,19536,19505,19494,19574,19607,19532,19599,19647,19661,19638,19666,19765,19779,19788,19813,19768,19732,19765,19751,19744,19803,19686,19721,19818,19782,19857,19792,19789,19920,19965,19894,19931,19969,19938,20046,20044,19966,20062,20162,20121,20134,20091,20112,20190,20117,20156,20209,20215,20296,20241,20278,20240,20311,20324,20288,20277,20288,20327,20319,20366,20369,20386,20420,20382,20450,20472,20505,20458,20545,20409,20444,20348,20219,20150,19942,19865,19841,19686,19671,19590,19499,19513,19532,19447,19519,19478,19453,19466,19517,19458,19508,19489,19511,19615,19593,19646,19673,19678,19729,19675,19697,19759,19687,19679,19758,19725,19727,19678,19691,19747,19702,19704,19817,19758,19761,19801,19826,19880,19898,19941,19923,19964,19930,19963,20021,19980,20045,20050,20076,20102,20107,20203,20159,20123,20184,20144,20135,20171,20189,20246,20211,20180,20268,20315,20280,20324,20298,20344,20370,20333,20409,20393,20385,20436,20390,20443,20472,20449,20313,20225,20072,20012,19868,19770,19605,19560,19550,19541,19534,19470,19600,19511,19503,19498,19468,19451,19471,19542,19580,19510,19553,19609,19671,19669,19714,19684,19719,19671,19736,19756,19752,19716,19762,19724,19784,19751,19759,19790,19801,19789,19767,19850,19834,19857,19917,19904,19937,19938,19912,20024,19969,19961,20036,20044,19995,20083,20059,20101,20120,20037,20154,20143,20102,20278,20223,20226,20308,20206,20199,20283,20298,20275,20302,20328,20312,20328,20301,20265,20292,20191,20088,19954,19816,19849,19618,19647,19635,19658,19523,19570,19520,19627,19559,19565,19554,19606,19577,19669,19617,19677,19718,19715,19814,19831,19848,19902,19830,19895,19887,19827,19849,19895,19867,19862,19937,19925,19922,19997,19906,20020,19968,20053,20017,20065,20085,20184,20118,20131,20167,20231,20195,20258,20195,20266,20272,20224,20329,20349,20318,20343,20322,20423,20361,20453,20487,20352,20395,20481,20534,20483,20482,20509,20528,20499,20500,20528,20476,20425,20348,20253,20131,20053,19969,19907,19794,19758,19781,19638,19711,19693,19610,19636,19667,19677,19691,19688,19765,19768,19697,19763,19817,19788,19783,19819,19915,19971,19915,19923,19968,19963,19915,19906,19912,19992,19952,19913,19935,20024,19960,20003,20019,20018,20090,20076,20113,20185,20107,20186,20199,20222,20261,20237,20232,20350,20313,20253,20383,20310,20378,20351,20336,20382,20410,20407,20465,20448,20363,20519,20424,20514,20509,20501,20568,20535,20546,20630,20597,20580,20587,20468,20520,20343,20207,20113,20070,19959,19911,19903,19765,19770,19759,19769,19766,19696,19755,19792,19730,19778,19707,19742,19796,19863,19824,19900,19913,19866,19964,19976,19961,19961,19921,19925,20000,19941,19968,19970,19941,20061,19972,19989,20020,20002,20090,20080,20085,20088,20112,20035,20184,20111,20157,20246,20229,20226,20242,20194,20291,20278,20275,20350,20280,20316,20362,20313,20318,20336,20267,20423,20376,20393,20433,20390,20495,20446,20453,20471,20490,20497,20591,20547,20575,20641,20637,20598,20555,20427,20367,20341,20169,20088,19960,19883,19817,19793,19715,19783,19678,19752,19717,19684,19736,19687,19698,19754,19755,19719,19797,19687,19768,19873,19851,19873,19947,19853,19965,19883,19891,19960,19888,19902,19906,19904,19958,19976,19844,19937,20023,19990,19998,20001,19956,20095,20036,20071,20084,20135,20129,20140,20181,20223,20238,20170,20205,20223,20243,20295,20284,20290,20383,20356,20290,20300,20338,20385,20315,20365,20458,20427,20449,20455,20450,20470,20469,20416,20540,20600,20523,20574,20580,20539,20483,20383,20242,20225,20057,19981,19943,19801,19772,19703,19750,19725,19717,19718,19760,19760,19739,19696,19784,19860,19798,19805,19949,19931,19999,20016,19982,20019,20046,19967,20046,20019,19988,20054,19968,20001,20022,20071,20067,20046,20104,20110,20128,20123,20176,20199,20155,20264,20267,20261,20329,20274,20277,20319,20359,20329,20324,20434,20415,20448,20494,20398,20467,20500,20390,20521,20532,20526,20522,20573,20587,20621,20587,20586,20656,20701,20670,20750,20675,20680,20693,20625,20523,20438,20343,20199,20077,20055,19985,19911,19977,19878,19828,19921,19869,19898,19854,19944,19857,19908,19964,20006,20006,19990,20100,20056,20171,20074,20155,20178,20181,20175,20181,20188,20175,20245,20204,20233,20182,20186,20255,20257,20300,20281,20328,20309,20436,20367,20365,20410,20434,20430,20433,20467,20601,20485,20596,20522,20575,20602,20607,20606,20735,20675,20708,20711,20682,20731,20770,20743,20802,20752,20818,20827,20780,20828,20820,20842,20963,20922,20818,20972,20879,20871,20955,20819,20757,20638,20524,20389,20270,20215,20214,20141,20077,20096,20037,19997,20021,20018,20083,20052,20026,20005,20031,20068,20056,20102,20178,20217,20191,20197,20254,20278,20227,20251,20120,20240,20211,20223,20323,20243,20290,20273,20281,20287,20291,20216,20358,20305,20347,20428,20425,20431,20433,20359,20444,20476,20467,20521,20537,20579,20591,20507,20554,20602,20613,20679,20677,20649,20719,20613,20687,20710,20660,20728,20724,20737,20752,20697,20784,20752,20831,20864,20822,20873,20896,20931,20945,20904,20897,20908,20837,20767,20589,20516,20434,20268,20202,20167,20096,20078,20009,20019,19990,19956,20026,19937,19988,19905,19942,19943,20002,20038,20060,20040,20093,20136,20043,20207,20071,20170,20147,20103,20190,20172,20159,20182,20107,20134,20174,20130,20184,20190,20217,20247,20276,20257,20277,20230,20359,20385,20367,20406,20462,20377,20423,20361,20445,20452,20550,20471,20488,20601,20510,20558,20605,20589,20581,20730,20615,20669,20628,20652,20668,20580,20659,20718,20730,20698,20741,20810,20847,20758,20759,20784,20750,20860,20804,20718,20696,20540};
		ArrayList<Double> signalD  = new ArrayList<>();
		for (int i=0;i<signal.length;i++){
			signalD.add(signal[i]*-1);
//			signalD.add(signal[i]);
		}
		ArrayList<Double> b = new ArrayList<>();
		b.add(0.0001832160233696091);
		b.add(0d);
		b.add(-0.0007328640934784364);
		b.add(0d);
		b.add( 0.0010992961402176546);
		b.add(0d);
		b.add(-0.0007328640934784364);
		b.add(0d);
		b.add(0.0001832160233696091);

		ArrayList<Double> a = new ArrayList<>();
		a.add(1d);
		a.add(-7.2713207413547964 );
		a.add(23.215531796334794);
		a.add(-42.512647480526255);
		a.add(48.84050016164899);
		a.add(-36.048507171305076);
		a.add(  16.693851471344185);
		a.add(-4.434886125119121);
		a.add(0.5174781997880);

		ArrayList<Double> result = doFiltfilt(b,a,signalD);
		for (int i=0;i<result.size();i++){
			System.out.printf(result.get(i)+",");
		}
	}

	/**
	 * 供外部调用的滤波方法
	 * @param signal 信号
	 * @return
	 */
	public static ArrayList<Double> doBandFilter(ArrayList<Double> signal){
		ArrayList<Double> b = new ArrayList<>();
		b.add(0.00156701035058827);
		b.add(0.0);
		b.add(-0.00470103105176481);
		b.add(0.0);
		b.add(0.00470103105176481);
		b.add(0.0);
		b.add(-0.00156701035058827);


		ArrayList<Double> a = new ArrayList<>();
		a.add(1.0);
		a.add(-5.474076076440611 );
		a.add(12.521046040867494);
		a.add(-15.321672780118373);
		a.add(10.58065381912825);
		a.add(-3.9100601507349713);
		a.add(0.6041096995072747);

		ArrayList<Double> result = doFiltfilt(b,a,signal);
		return result;
	}

	/**
	 * 
	 * @param B
	 *            滤波器参数B
	 * @param A
	 *            滤波器参数A
	 * @param X
	 *            要滤波的数组 ： 要求是一维，长度要大于B数组。
	 * @return 滤波后的结果 返回double数组。
	 */
	public static ArrayList<Double> doFiltfilt(ArrayList<Double> B,
			ArrayList<Double> A, ArrayList<Double> X) {

		int len = X.size();
		int na = A.size();
		int nb = B.size();
		int nfilt = (nb > na) ? nb : na;
		int nfact = 3 * (nfilt - 1);
		if (len <= nfact)
			throw new RuntimeException("输入数值X长度太小，数据最少是滤波器阶数的三倍");
		resize(B,nfilt, 0);
		resize(A,nfilt, 0);
		
		ArrayList<Integer> rows = new ArrayList<Integer>();
		ArrayList<Integer> cols = new ArrayList<Integer>();

		add_index_range(rows, 0, nfilt - 2,1);
		if (nfilt > 2)
		{
			add_index_range(rows, 1, nfilt - 2, 1);
			add_index_range(rows, 0, nfilt - 3, 1);
		}
		add_index_const(cols, 0, nfilt - 1);
		if (nfilt > 2)
		{
			add_index_range(cols, 1, nfilt - 2,1);
			add_index_range(cols, 1, nfilt - 2,1);
		}
		int klen = rows.size();
		ArrayList<Double> data = new ArrayList<Double>();
		resize(data,klen, 0);
		data.set(0, 1 + A.get(1));
		int j = 1;
		if (nfilt > 2)
		{
			for (int i = 2; i < nfilt; i++)
				data.set(j++, A.get(i));
			for (int i = 0; i < nfilt - 2; i++)
				data.set(j++, 1.0);
			for (int i = 0; i < nfilt - 2; i++)
				data.set(j++, -1.0);
		}
		ArrayList<Double> leftpad = subvector_reverse(X, nfact, 1);
		changeArray2(leftpad,2*X.get(0));
		
		ArrayList<Double> rightpad = subvector_reverse(X, len - 2, len - nfact - 1);
		changeArray2(rightpad,2*X.get(len - 1));
		
		double y0;
		ArrayList<Double> signal1 = new ArrayList<Double>();
		ArrayList<Double> signal2 = new ArrayList<Double>();
		ArrayList<Double> zi = new ArrayList<Double>();
		//reserve(signal1,leftpad.size() + X.size() + rightpad.size(),0);
		append_vector(signal1, leftpad);
		append_vector(signal1, X);
		append_vector(signal1, rightpad);
		
		double [][] sp = Zeros(max_val(rows)+1,max_val(cols)+1); 
		for (int k = 0; k < klen; ++k)
			sp[rows.get(k)][cols.get(k)] = data.get(k);
		double[]bb = map(B);
		double[]aa = map(A);
		//Mat.inv(sp)
		//bb.segment(1, nfilt - 1) - (bb(0) * aa.segment(1, nfilt - 1))
		
		double[][] ZZi = Mat.multi(Mat.inv(sp),calc(segment(bb,1,nfilt - 1),bb[0],segment(aa,1,nfilt - 1)));
		
		resize(zi,ZZi.length,1);
		
		changeZi(ZZi,zi,signal1.get(0));
		filter(B, A, signal1, signal2, zi);
		reverse(signal2);
		changeZi(ZZi,zi,signal2.get(0));
		filter(B, A, signal2, signal1, zi);
		ArrayList<Double> Y = subvector_reverse(signal1, signal1.size() - nfact - 1, nfact);
		return Y;

	}

	private static void reverse(ArrayList<Double> signal2) {
		int i=0;
		int j=signal2.size()-1;
		while(i<j){
			swap(signal2,i,j);
			i++;
			j--;
		}
	}

	private static void swap(ArrayList<Double> signal2, int i, int j) {
		double temp = signal2.get(j);
		signal2.set(j, signal2.get(i));
		signal2.set(i, temp);
	}

	private static void changeZi(double[][] zZi, ArrayList<Double> zi,
			Double double1) {
		for (int i = 0; i < zZi.length; i++) {
			zi.set(i, zZi[i][0]*double1);
		}
		
	}

	private static double[][] calc(double[] segment, double d, double[] segment2) {
		double[][] ret = new double[segment.length][1];
		for (int i = 0; i < segment.length; i++) {
			ret[i][0] = segment[i]-d*segment2[i];
		}
		return ret;
	}

	private static double[] segment(double[] bb, int i, int j) {
		double[]ret=new double[j-i+1];
		for (int k = 0; k < j-i+1; k++) {
			ret[k] = bb[i+k];
		}
		return ret;
	}

	private static double[] map(ArrayList<Double> b) {
		double[] ret = new double[b.size()];
		for (int i = 0; i < ret.length; i++) {
			ret[i] = b.get(i);
		}
		return ret;
	}

	private static double[][] Zeros(int ii, int jj) {
		double [][] sp = new double[ii][jj];
		for (int i = 0; i < ii; i++)
			for (int j = 0; j < jj; j++)
				sp[i][j] = 0;
		return sp;
	}

	public static void filter(ArrayList<Double> B, ArrayList<Double> A,
			ArrayList<Double> X, ArrayList<Double> Y,ArrayList<Double> Zi) {
		if (A.size() == 0)
			throw new RuntimeException("A 数组为空！");
		boolean flagA = true;
		for (Double doubleA : A) {
			if (doubleA != 0) {
				flagA = false;
			}
		}
		if (flagA) {
			throw new RuntimeException("A 数组至少要有一个数不为零！");
		}
		if (A.get(0) == 0) {
			throw new RuntimeException("A 数组第一个元素不能为零！");
		}
		double a0 = A.get(0);
		changeArray(A, a0);
		changeArray(B, a0);
		
		
		int input_size = X.size();
		int filter_order = max(A.size(),B.size());
		resize(B,filter_order,0);
		resize(A,filter_order,0);
		resize(Zi,filter_order,0);
		resize(Y,input_size,0);
		
		for (int i = 0; i < input_size; i++) {
			int order = filter_order - 1;
			while(order!=0){
				if(i>=order)
					Zi.set(order-1, B.get(order)*X.get(i-order)-A.get(order)*Y.get(i-order)+Zi.get(order));
				--order;
			}
			Y.set(i, B.get(0)*X.get(i)+Zi.get(0));
		}
		Zi.remove(Zi.size()-1);		
	}

	

	private static void resize(ArrayList<Double> a, int i, double j) {
		if(a.size()>=i)
			return;
		int size = a.size();
		for (int j2 = size; j2 < i; j2++) {
			a.add(j);
		}
	}

	private static int max(int size, int size2) {
		if(size >size2)
			return size;
		else
			return size2;
	}

	static void changeArray(ArrayList<Double> vec, double a0) {
		for (int i = 0; i < vec.size(); i++) {
			vec.set(i, vec.get(i)/a0);
		}
	}
	
	static void changeArray2(ArrayList<Double> vec, double a0) {
		for (int i = 0; i < vec.size(); i++) {
			vec.set(i, a0-vec.get(i));
		}
	}

	static void add_index_range(ArrayList<Integer> indices, int beg, int end,
			int inc) {
		for (int i = beg; i <= end; i += inc)
			indices.add(i);
	}

	static void add_index_const(ArrayList<Integer> indices, int value, int numel) {
		while (numel-- != 0)
			indices.add(value);
	}

	static void append_vector(ArrayList<Double> vec, ArrayList<Double> tail) {
		for (Double doubleitem : tail) {
			vec.add(doubleitem);
		}
	}

	static ArrayList<Double> subvector_reverse(ArrayList<Double> vec,
			int idx_end, int idx_start) {
		ArrayList<Double> resultArrayList = new ArrayList<Double>(idx_end
				- idx_start + 1);
		for (int i = 0; i < idx_end- idx_start + 1; i++) {
			resultArrayList.add(0.0);
		}
		int endindex = idx_end - idx_start;
		for (int i = idx_start; i <= idx_end; i++)
			resultArrayList.set(endindex--, vec.get(i));
		return resultArrayList;
	}

	static int max_val(ArrayList<Integer> vec) {
		int temp = vec.get(0);
		for (Integer integer : vec) {
			if (temp < integer)
				temp = integer;
		}
		return temp;
	}

}
